#include "Keyboard.h"
#include <vector>

// USBHIDMouse eps32_Mouse;
USBHIDKeyboard eps32_Keyboard;
SPIClass *vspi = NULL;
Nvs nvs1;
NecPixel pixelrgb;


uint8_t old_LRBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};
uint8_t LRBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};

uint8_t Keyboard::spiBuffer[IO_NUMBER / 8 + 1] = {0, 0, 0, 0};
uint8_t Keyboard::old_spiBuffer[IO_NUMBER / 8 + 1] = {0, 0, 0, 0};
uint8_t Keyboard::debounceBuffer[IO_NUMBER / 8 + 1] = {0, 0, 0, 0};
uint8_t Keyboard::remapBuffer[HC_NUMBER] = {0, 0, 0, 0, 0, 0, 0, 0};
uint8_t *Keyboard::scanBuffer = LRBuffer;
uint8_t *Keyboard::pspiBuffer = spiBuffer;

int16_t hidBuffer[6] = {0, 0, 0, 0, 0, 0};
int16_t old_hidBuffer[6] = {0, 0, 0, 0, 0, 0};

uint8_t SlaveBuffer[7] = {0, 0, 0, 0, 0, 0, 0};
uint8_t broadcastAddress_r[] = {0xDC, 0xDA, 0x0C, 0x17, 0xD3, 0xC8};
uint8_t broadcastAddress_l[] = {0x74, 0x4D, 0xBD, 0xA1, 0xA0, 0x40};

int16_t Keyboard::keyMap[5][HC_NUMBER * 8] = {
    {
        31,30,25,24,26,27, /**/ 59,58,56,57,62,63,
        22,16,18,19,28,29, /**/ 61,60,51,50,48,49,
        8,10,11,20,21,23, /**/ 54,53,55,52,46,47,
        3,12,13,15,14,9, /**/ 43,42,40,41,33,38,
        7,6, /**/ 34,32,
        1,0,2,/**/ 45,44,35,
        4,5,17,36,37,39

    },
    {GRAVE_ACCENT, NUM_1, NUM_2, NUM_3, NUM_4, NUM_5, /**/ NUM_6, NUM_7, NUM_8, NUM_9, NUM_0, NULL,
     TAB, Q, W, E, R, T, /**/ Y, U, I, O, P, BACKSLASH,
     CAP_LOCK, A, S, D, F, G, /**/ H, J, K, L, SEMI_COLON, QUOTE,
     KEY_LEFT_GUI, Z, X, C, V, B, /**/ N, M, COMMA, PERIOD, SLASH, NULL,
     LEFT_U_BRACE, RIGHT_U_BRACE, /**/ MINUS, EQUAL,
     DELETE, BACKSPACE, FN,/**/  ESC, SPACE,ENTER,
     NULL, NULL, NULL, NULL, NULL, NULL},
     {GRAVE_ACCENT, NUM_1, NUM_2, NUM_3, NUM_4, NUM_5, /**/ NUM_6, NUM_7, NUM_8, NUM_9, NUM_0, NULL,
     TAB, Q, W, E, R, T, /**/ Y, U, I, O, P, BACKSLASH,
     CAP_LOCK, A, S, D, F, G, /**/ H, J, K, L, SEMI_COLON, QUOTE,
     KEY_LEFT_GUI, Z, X, C, V, B, /**/ N, M, COMMA, PERIOD, SLASH, NULL,
     LEFT_U_BRACE, RIGHT_U_BRACE, /**/ MINUS, EQUAL,
     KEY_LEFT_SHIFT, KEY_LEFT_CTRL, FN,/**/  KEY_RIGHT_ALT, KEY_RIGHT_CTRL,KEY_RIGHT_SHIFT,
     NULL, NULL, NULL, NULL, NULL, NULL},
    {GRAVE_ACCENT, F1, F2, F3, F4, F5, /**/ F6, F7, F8, F9, F10, F11,
     TAB, Q, W, E, R, T, /**/ Y, U, UP_ARROW, O, P, F12,
     CAP_LOCK, A, S, D, F, G, /**/ H, LEFT_ARROW, DOWN_ARROW, RIGHT_ARROW, SEMI_COLON, F13,
     KEY_LEFT_GUI, Z, X, C, V, B, /**/ HOME, END, PAGE_UP, PAGE_DOWN, SLASH, NULL,
     LEFT_U_BRACE, RIGHT_U_BRACE, /**/ MINUS, EQUAL,
     KEY_LEFT_SHIFT, KEY_LEFT_CTRL, FN,/**/  KEY_RIGHT_ALT, KEY_RIGHT_CTRL,KEY_RIGHT_SHIFT,
     NULL, NULL, NULL, NULL, NULL, NULL},
};

SemaphoreHandle_t key = NULL;     
TickType_t timeOut = 1000; 
uint8_t rgbshowtype = 1;
bool rgbinitflag = false;

int8_t LregMap[2][32] = {
    {
        31,30,25,24,26,27,
        22,16,18,19,28,29,
        8,10,11,20,21,23, 
        3,12,13,15,14,9, 
        7,6,
        1,0,2,
        4,5,17
    },
    {
        24,23,15,14,6,5,
        25,22,16,13,7,4,
        26,21,17,12,8,3,
        27,20,18,11,9,2,
          19,10,
             1,0,-1,
        -1,-1,-1
    }
};

int8_t RregMap[2][32] = {
    {
        27,26,24,25,30,31,
        29,28,19,18,16,17,
        22,21,23,20,14,15,
        11,10,8,9,1,6,
        2,0,
        13,12,3,
        4,5,7

    },
    {
        5,6,14,15,23,24,
        4,7,13,16,22,25,
        3,8,12,17,21,26,
        2,9,11,18,20,27,
           10,19,
        -1,0,1,
        -1,-1,-1
    }
};

struct RGBlb
{
    int8_t led_num;
    int8_t Brightness;
};

#define rgbLength 10
struct RGBlb rgbs[rgbLength] = {{-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}, {-1, -1}};

// uint8_t broadcastAddress[] = {0xDC, 0x54, 0x75, 0xDF, 0x16, 0xC4};
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status);
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len);
void static press();
void static releaseAll();
void addrgbs(int8_t value);
RGBlb dislodgergbs(int8_t index);
void deletergbs(int8_t index);

TaskHandle_t handleTaskLED;

Keyboard::Keyboard(/* args */)
{
}

Keyboard::~Keyboard()
{
}

void Keyboard::keyboard_init(void *pvParameters)
{
    vspi = new SPIClass(FSPI);
    // vspi->begin();
    vspi->begin(VSPI_SCLK, VSPI_MISO, VSPI_MOSI, VSPI_SS); // SCLK, MISO, MOSI, SS
    Serial.print("pinSS:");
    Serial.println(vspi->pinSS());
    pinMode(vspi->pinSS(), OUTPUT); // VSPI SS
    pinMode(VSPI_CE, OUTPUT);
    digitalWrite(VSPI_CE, HIGH);

    eps32_Keyboard.begin();
    USB.serialNumber("a");
    USB.manufacturerName("l");
    USB.productName("p");
    USB.begin();

    WiFi.mode(WIFI_MODE_STA);
    WiFi.disconnect();
    ESPNow.init();
    ESPNow.add_peer(broadcastAddress_r);
    ESPNow.reg_recv_cb(OnDataRecv);
    key = xSemaphoreCreateMutex();
    xTaskCreatePinnedToCore(
        showRGB,
        "showRGB",
        6144, // 栈大小
        NULL,
        15, // 任务优先级, with 3 (configMAX_PRIORITIES - 1) 是最高的，0是最低的.
        &handleTaskLED,
        1);

    while (1)
    {
        memcpy(old_hidBuffer, hidBuffer, sizeof(hidBuffer));
        memcpy(old_spiBuffer, spiBuffer, sizeof(spiBuffer));
        ScanKeyStates();
        ApplyDebounceFilter(1);

        memcpy(old_LRBuffer, LRBuffer, sizeof(LRBuffer));
        memset(LRBuffer, 0xFF, 8);

        uint8_t index = 0;
        for (size_t i = 0; i < 4; i++)
        {
            LRBuffer[index] = spiBuffer[index];
            index++;
        }
        if (SlaveBuffer[1] & 0xf && SlaveBuffer[6] & 0xf0)
        {
            for (size_t i = 0; i < 4; i++)
            {
                LRBuffer[index] = SlaveBuffer[i + 2];
                index++;
            }
        }
        scanBuffer = LRBuffer;
        
#if DEBUG
       
        
        //Serial.print("    scanBuffer:");
        //for (size_t i = 0; i < 8; i++)
        //{
        //    Serial.print(scanBuffer[i], BIN);
        //    Serial.print("    ");
        //}
        //Serial.println("    ");

        Serial.print("old_LRBuffer:");
        for (size_t i = 0; i < 8; i++)
        {
            Serial.print(old_LRBuffer[i], BIN);
            Serial.print("    ");
        }
        Serial.println("    ");
        Serial.print("    LRBuffer:");
        for (size_t i = 0; i < 8; i++)
        {
            Serial.print(LRBuffer[i], BIN);
            Serial.print("    ");
        }
        Serial.println("    ");
         vTaskDelay(1000);
#endif
        
        bool downflag = true;
        for (size_t i = 0; i < sizeof(LRBuffer) / sizeof(LRBuffer[0]); i++)
        {
            if (LRBuffer[i] != old_LRBuffer[i])
            {
                if (LRBuffer[i] != 0xFF)
                {
                    downflag = false;
                }
            }
        }
        if (!(LRBuffer[0] & 0x20) && !(LRBuffer[4] & 0x2))
        {
            if (rgbshowtype == 2)
                rgbshowtype = 0;
            else
                rgbshowtype++;

            vTaskDelay(2000);
            
            rgbinitflag = true;
            Serial.print("rgbshowtype");
            Serial.println(rgbshowtype);
        }
        if (!(LRBuffer[0] & 0x20) && (LRBuffer[0] & 0x40 && LRBuffer[0] & 0x80 && LRBuffer[5] & 0x4 && LRBuffer[5] & 0x8 && LRBuffer[4] & 0x10))
        {
            Remap(3);
            Serial.print(" Remap(3)");
        }
        else if (!(LRBuffer[0] & 0x20) && !(LRBuffer[0] & 0x40 && LRBuffer[0] & 0x80 && LRBuffer[5] & 0x4 && LRBuffer[5] & 0x8 && LRBuffer[4] & 0x10))
        {
            Remap(2);
        }
        else
        {
            Remap(1);
        }

        if (KeyPressed(R) && KeyPressed(NUM_5))
        {
            // do something...
            // or trigger some keys...
        }
        release();

        if (downflag)
        {
#if DEBUG
            //Serial.println("结束本次轮询");
#endif
            continue;
        }
#if DEBUG
        Serial.println("开始映射");
#endif
        

        press();
    }
    vTaskDelete(NULL);
}

/// @brief 右键盘传输接收
/// @param pvParameters
void Keyboard::transmission(void *pvParameters)
{
    vspi = new SPIClass(FSPI);
    // vspi->begin();
    vspi->begin(VSPI_SCLK, VSPI_MISO, VSPI_MOSI, VSPI_SS); // SCLK, MISO, MOSI, SS

    Serial.print("pinSS:");
    Serial.println(vspi->pinSS());
    pinMode(vspi->pinSS(), OUTPUT); // VSPI SS
    pinMode(VSPI_CE, OUTPUT);
    digitalWrite(VSPI_CE, HIGH);

    WiFi.mode(WIFI_MODE_STA);
    WiFi.disconnect();
    ESPNow.init();
    ESPNow.add_peer(broadcastAddress_l);
    ESPNow.reg_send_cb(OnDataSent);
    //ESPNow.reg_recv_cb(OnDataRecv);

    key = xSemaphoreCreateMutex();
    xTaskCreatePinnedToCore(
        showRGB,
        "showRGB",
        6144, // 栈大小
        NULL,
        15, // 任务优先级, with 3 (configMAX_PRIORITIES - 1) 是最高的，0是最低的.
        &handleTaskLED,
        1);

    while (true)
    {
        memcpy(old_spiBuffer, spiBuffer, sizeof(spiBuffer));
        ScanKeyStates();
        ApplyDebounceFilter(1);

        bool downflag = true;
        for (size_t i = 0; i < sizeof(spiBuffer) / sizeof(spiBuffer[0]); i++)
        {
            if (spiBuffer[i] != old_spiBuffer[i])
            {
                downflag = false;
            }
        }
        if ( downflag )
        {
            continue;
        }
        
        memset(SlaveBuffer, 0x00, 7);
        SlaveBuffer[0] = 0xf;
        SlaveBuffer[1] = IO_NUMBER / 8;

        for (size_t i = 0; i < IO_NUMBER / 8; i++)
        {
            SlaveBuffer[i + 2] = spiBuffer[i];
        }
        SlaveBuffer[IO_NUMBER / 8 + 2] = 0xf0;

        esp_err_t result = ESPNow.send_message(broadcastAddress_l, (uint8_t *)&SlaveBuffer, sizeof(SlaveBuffer));
        if (!(spiBuffer[0] & 0x2))
        {
            if (rgbshowtype == 2)
                rgbshowtype = 0;
            else
                rgbshowtype++;

            vTaskDelay(2000);

            rgbinitflag = true;
            Serial.print("rgbshowtype");
            Serial.println(rgbshowtype);
        }

        vTaskDelay(10);
#if DEBUG

        Serial.print("    spiBuffer:");
        for (size_t i = 0; i < 5; i++)
        {
            Serial.print(spiBuffer[i], BIN);
            Serial.print("    ");
        }
        Serial.println("    ");
        Serial.print("old_spiBuffer:");
        for (size_t i = 0; i < 5; i++)
        {
            Serial.print(old_spiBuffer[i], BIN);
            Serial.print("    ");
        }
        Serial.println("    ");
        if (result == ESP_OK)
        {
            Serial.println("Sent with success");
        }
        else
        {
            Serial.print("Error sending the data");
            Serial.println(result);
        }
        vTaskDelay(1000);
#endif
    }
    vTaskDelete(NULL); // 删除自身函数
}

/// @brief 检查键的状态
/// @return
void *Keyboard::ScanKeyStates()
{
    memset(spiBuffer, 0xff, IO_NUMBER / 8 + 1);

    digitalWrite(vspi->pinSS(), LOW);  // pull SS slow to prep other end for transfer
    digitalWrite(vspi->pinSS(), HIGH); // pull ss high to signify end of data transfer
    vspi->beginTransaction(SPISettings(spiClk, LSBFIRST, SPI_MODE2));

    digitalWrite(VSPI_CE, LOW);
    for (int i = 0; i < IO_NUMBER / 8 + 1; i++)
    {
        spiBuffer[i] = vspi->transfer(0);
    }

    digitalWrite(VSPI_CE, HIGH);
    vspi->endTransaction();

    // scanBuffer = spiBuffer;

    return spiBuffer;
}

/// @brief 滤波
/// @param _filterTimeUs
void Keyboard::ApplyDebounceFilter(uint32_t _filterTimeUs)
{
    memcpy(debounceBuffer, spiBuffer, IO_NUMBER / 8 + 1);

    vTaskDelay(_filterTimeUs);
    ScanKeyStates();

    uint8_t mask;
    for (int i = 0; i < IO_NUMBER / 8 + 1; i++)
    {
        mask = debounceBuffer[i] ^ spiBuffer[i];
        spiBuffer[i] |= mask;
    }
    if(xSemaphoreTake(key, timeOut) == pdPASS)
    {
        pspiBuffer = spiBuffer;
        xSemaphoreGive(key);
    }
    
}

/// @brief 映射键
/// @param _layer
/// @return
uint8_t *Keyboard::Remap(uint8_t _layer)
{
#if DEBUG
    // Serial.print("keylayerSN :");
    // Serial.println(_layer);
#endif
    int16_t index, bitIndex;

    memset(remapBuffer, 0x00, sizeof(remapBuffer));
    memset(hidBuffer, 0x00, sizeof(hidBuffer));
    for (int16_t i = 0; i < HC_NUMBER; i++)
    {
        for (int16_t j = 0; j < 8; j++)
        {
            index = (int16_t)(keyMap[0][i * 8 + j] / 8);
            bitIndex = (int16_t)(keyMap[0][i * 8 + j] % 8);

            if (scanBuffer[index] & (0x80 >> bitIndex))
            {
                remapBuffer[i] |= 0x80 >> j;
            }
        }
        remapBuffer[i] = ~remapBuffer[i];
    }
    int i = 0, j = 0, hidindex = 0;
    while (8 * i + j < HC_NUMBER * 8)
    {
        for (j = 0; j < 8; j++)
        {
            index = (int16_t)(keyMap[_layer][i * 8 + j] / 8 + 1); // +1 for modifier
            bitIndex = (int16_t)(keyMap[_layer][i * 8 + j] % 8);
            if (bitIndex < 0)
            {
                index -= 1;
                bitIndex += 8;
            }
            else if (index > 100)
                continue;

            if (remapBuffer[i] & (0x80 >> j))
            {
                hidBuffer[hidindex] = keyMap[_layer][i * 8 + j];
                hidindex++;
                hidBuffer[hidindex + 1] = '\0';
            }
        }
        i++;
        j = 0;
    }

    return remapBuffer;
}

/// @brief 释放键
void Keyboard::release()
{
    std::vector<int> arrayA(hidBuffer, hidBuffer + 6);
    std::vector<int> arrayB(old_hidBuffer, old_hidBuffer + 6);
    std::vector<int> diff;
    std::set_difference(arrayB.begin(), arrayB.end(), arrayA.begin(), arrayA.end(), std::back_inserter(diff));
#if DEBUG
    Serial.print("arrayA:");
    for (size_t i = 0; i < arrayA.size(); i++)
    {
        Serial.print(" ");
        Serial.print(arrayA[i]);
    }
    Serial.println();
    Serial.print("arrayB:");
    for (size_t i = 0; i < arrayB.size(); i++)
    {
        Serial.print(" ");
        Serial.print(arrayB[i]);
    }
    Serial.println();
    Serial.print("diff:");
    for (size_t i = 0; i < diff.size(); i++)
    {
        Serial.print(" ");
        Serial.print(diff[i]);
    }
    Serial.println();
#endif
    for (size_t i = 0; i < diff.size(); i++)
    {
        eps32_Keyboard.release(diff[i]);
    }
}

/// @brief 是否被摁下
/// @param _key
/// @return
bool Keyboard::KeyPressed(KeyCode_t _key)
{
    bool flag = false;
    for (size_t i = 0; i < 6; i++)
    {
        if (hidBuffer[i] == _key)
        {
            flag = true;
            break;
        }
    }
    return flag;
}

/// @brief rgb
/// @param pvParameters 
void Keyboard::showRGB(void *pvParameters){
    
    
    while (1)
    {
        if (millis() < 3000)
        {
            rgbinitflag = true;
            pixelrgb.rainbow();
            pixelrgb.show();
            vTaskDelay(1000);
            continue;
        }
        if (rgbinitflag == true)
        {
            Serial.println("clear");
            pixelrgb.setBrightness(defBrightness);
            pixelrgb.clear(1);
            rgbinitflag = false;
        }

        if (rgbshowtype == Breathing)
        {
            pixelrgb.Breathing();
            pixelrgb.show();
            vTaskDelay(100);
        }
        else if (rgbshowtype == blink)
        {
            if (xSemaphoreTake(key, timeOut) == pdPASS)
            {
                uint8_t rgbBuffer[4] = {0, 0, 0, 0};
                int16_t index, bitIndex;
                memset(rgbBuffer, 0x00, sizeof(rgbBuffer));
                //Serial.print("pspiBuffer :");
                for (int16_t i = 0; i < 4; i++)
                {
                    for (int16_t j = 0; j < 8; j++)
                    {
#if IShost
                        index = (int16_t)(LregMap[0][i * 8 + j] / 8);
                        bitIndex = (int16_t)(LregMap[0][i * 8 + j] % 8);

#else
                        index = (int16_t)(RregMap[0][i * 8 + j] / 8);
                        bitIndex = (int16_t)(RregMap[0][i * 8 + j] % 8);
#endif
                        if (pspiBuffer[index] & (0x80 >> bitIndex))
                        {
                            rgbBuffer[i] |= 0x80 >> j;
                        }
                    }
                    
                    //Serial.print(pspiBuffer[i],BIN);
                    //Serial.print("  ");
                    rgbBuffer[i] = ~rgbBuffer[i];
                }
                int i = 0, j = 0, hidindex = 0;
                while (8 * i + j < 4 * 8)
                {
                    for (j = 0; j < 8; j++)
                    {
                        if (rgbBuffer[i] & (0x80 >> j))
                        {
#if IShost
                            addrgbs(LregMap[1][8 * i + j]);
#else
                            addrgbs(RregMap[1][8 * i + j]);
#endif
                        }
                    }
                    i++;
                    j = 0;
                }
                xSemaphoreGive(key);
            }

            for (size_t i = 0; i < rgbLength; i++)
            {
                RGBlb rgb = dislodgergbs(i);
                if (rgb.Brightness == 0)
                {
                    i--;
                }
                pixelrgb.blink(rgb.led_num, rgb.Brightness);
            }
            pixelrgb.show();
            vTaskDelay(50);
        }
        else if (rgbshowtype == rainbowCycle)
        {
            pixelrgb.rainbowCycle();
        }
    }
    vTaskDelete(NULL);
}

/// @brief 摁键
void static press()
{
    for (uint8_t i = 0; i < 6; i++)
    {
        Serial.print("press");
        Serial.print(i);
        Serial.print(":");
        Serial.println(hidBuffer[i]);

        if (hidBuffer[i] == '\0' || hidBuffer[i] == 0 || hidBuffer[i] == 1000)
        {
            continue;
        }

        eps32_Keyboard.press(hidBuffer[i]);
    }
}

/// @brief 全键释放
void static releaseAll()
{
    if (keyboardtype == 1)
        eps32_Keyboard.releaseAll();
    else
        Serial.println("未找到对应模式");
}

/// @brief 回调函数，在发送消息时执行
/// @param mac_addr
/// @param status
void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status)
{
#if DEBUG
    Serial.print("SlaveBuffer: ");
    for (size_t i = 0; i < sizeof(SlaveBuffer); i++)
    {
        Serial.print(SlaveBuffer[i], BIN);
        Serial.print(" ");
    }
    Serial.println();
    Serial.print("\r\nLast Packet Send Status:\t");
    Serial.println(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
#endif
}

/// @brief 回调函数，在接收消息时执行
/// @param mac
/// @param incomingData
/// @param len
void OnDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len)
{

    memcpy(&SlaveBuffer, incomingData, sizeof(SlaveBuffer));
#if DEBUG
    Serial.print("SlaveBuffer: ");
    for (size_t i = 0; i < sizeof(SlaveBuffer); i++)
    {
        Serial.print(SlaveBuffer[i], BIN);
        Serial.print(" ");
    }
    Serial.println();
#endif
}

/// @brief 添加
/// @param value 
void addrgbs(int8_t value)
{
    for (size_t i = 0; i < rgbLength; i++)
    {
        if (rgbs[i].led_num == -1 && rgbs[i].Brightness == -1 )
        {
            rgbs[i] = {value,20};
            break;
        }
    }
    
}
/// @brief 取出
/// @param index 
RGBlb dislodgergbs(int8_t index)
{
    RGBlb dom = rgbs[index];
    if ((rgbs[index].Brightness - 1) == -1)
    {
        deletergbs(index);
    }
    if (rgbs[index].Brightness != -1)
    {
        rgbs[index].Brightness--;
    }
    return dom;

}

/// @brief 删除
/// @param index 
void deletergbs(int8_t index)
{
    rgbs[index] = {-1,-1};
    for (size_t i = index; i < rgbLength; i++)
    {
        if (i + 1 < rgbLength)
        {
            rgbs[i] = rgbs[i + 1];
            rgbs[i + 1] = {-1, -1};
        }
    }
}